/* 
 * $Id$
 *
 * UMKC Robotics 2009
 *
 * This file provides the implementation for the path
 * builder.
**/

#include "err.h"
extern int err;

#include "path.h"
#include "2space.h"

#ifdef DBG
#include "iopublic.h"
extern const BotIoctl bioctl;
#endif


/* Just a decleration... */
int p_linear_segmentation(Path*);



/* function that builds the path.  This belongs in a different
 * file, but ok for now. */
extern TwoDOps mathops;
extern TwoD Upos;

int p_linear_segmentation(Path* p) {

  Triangle tbig, tsmall;
  int i, k;

  mathops.t_reset(&tbig);
  mathops.t_reset(&tsmall); 

  mathops.t_set_a(&tbig, 
              (p->y[END_VERTEX(p)] - p->y[START_VERTEX]) ); 
  mathops.t_set_b(&tbig, 
              (p->x[END_VERTEX(p)] - p->x[START_VERTEX]) );
  mathops.t_c_from_distanceFormula(&tbig);
  mathops.t_A_from_tanInv(&tbig);
  
  if( p->numverts < tbig.c )
    mathops.t_set_c(&tsmall, (tbig.c / p->numverts) );
  else if( p->numverts > tbig.c ) 
    mathops.t_set_c(&tsmall, (tbig.c * (1/p->numverts)) );
  else
    mathops.t_set_c(&tsmall, 1);

  mathops.t_set_A(&tsmall, tbig.A);
  mathops.t_a_from_csinA(&tsmall);
  mathops.t_b_from_ccosA(&tsmall);
 
  for(i=START_BUILD_VERTEX, k=1; i<=END_BUILD_VERTEX(p); i++, k++ ) {
    p->x[i] = p->x[i-1] + (k * tsmall.b);
    p->y[i] = p->y[i-1] + (k * tsmall.a);
    
  }

  return(E_NO_ERROR);

}
